package com.googlecode.gunncs.controller;

import com.googlecode.grtframework.core.GRTLoggedProcess;
import com.googlecode.grtframework.event.AccelerometerEvent;
import com.googlecode.grtframework.event.AccelerometerListener;
import com.googlecode.grtframework.sensor.IAccelerometer;
import com.googlecode.gunncs.actuator.RobotBase;

public class TiltController extends GRTLoggedProcess implements
		AccelerometerListener {

	private final RobotBase b;
	private final IAccelerometer acc;

	private double v;
	private double omega;

	public TiltController(RobotBase b, IAccelerometer acc, String name) {
		super(name);
		this.b = b;
		this.acc = acc;

	}

	@Override
	public void accelerationChanged(AccelerometerEvent e) {
		// TODO Auto-generated method stub

		switch (e.getAxis()) {
		case IAccelerometer.X:
			omega = -e.getAcceleration();
			break;
		case IAccelerometer.Y:
			v = e.getAcceleration();
			break;
		}

		b.drive(v, omega);
	}

	@Override
	public void initiate() {
		super.initiate();
		acc.addAccelerometerListener(this);
	}

	@Override
	public void terminate() {
		super.terminate();
		acc.removeAccelerometerListener(this);
	}
}
